#pragma config(Sensor, in1,    grabPotentiometer,   sensorPotentiometer)
#pragma config(Sensor, in4,    rightEncoder,        sensorQuadEncoder)
#pragma config(Sensor, dgtl1,  leftEncoder,         sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  rightEncoder,        sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  armEncoder,          sensorQuadEncoder)
#pragma config(Sensor, dgtl7,  autonStage,          sensorTouch)
#pragma config(Sensor, dgtl11, isNear,              sensorTouch)
#pragma config(Sensor, dgtl12, isRed,               sensorTouch)
#pragma config(Motor,  port1,           LIFT_MOTOR,    tmotorNormal, openLoop)
#pragma config(Motor,  port2,           RIGHT_WHEEL_MOTOR, tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           LEFT_WHEEL_MOTOR, tmotorNormal, openLoop)
#pragma config(Motor,  port4,           RIGHT_WHEEL_MOTOR, tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           LEFT_WHEEL_MOTOR, tmotorNormal, openLoop)
#pragma config(Motor,  port6,           LIFT_MOTOR,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           LEFT_LIFT_MOTOR, tmotorNormal, openLoop)
#pragma config(Motor,  port8,           RIGHT_WHEEL_MOTOR, tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port9,           LEFT_WHEEL_MOTOR, tmotorNormal, openLoop)
#pragma config(Motor,  port10,          LIFT_MOTOR,    tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings

#pragma autonomousDuration(1)
#pragma userControlDuration(2000)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

/*TODO to mikey's code:
* +++++++Put a numerator/denominator pair in pid function for each parameter
* +++++++Remove the error scaling
* +++++++Reset the accumulators after function is finished.
* +++++++Look for places in the I calculation where (long) casting may need to be done
* +++++++Put the calculateDriveSpeed function back into the while loop
* +++++++Determine driveStraight/driveTurn doneness using speed array.
* Calibrate driveTurn degree to target conversion.
* Write a liftArm function
  Calibrate the timing T2 for arm????
  +++++++Change the auto stages so they don't short circuit.  ie: take out the && and chain a stageEnd variable
  Write a drive slow that targets speed rather than distance.
  +++++++fix withinRange to include <=
  Set accumulator to 0 when error is sufficiently low.
  Write a drive function that optionally limits acceleration.
*/


/////////////////////////////////////////////////////////////////////////////////////////
//
//
//                                     DEFINES
//
//
//
/////////////////////////////////////////////////////////////////////////////////////////
#define ENCODERACCEPTANCERANGE 100
#define SPEEDACCEPTANCERANGE 5

#define DRIVE_KP 1
#define DRIVE_KPD 5
#define DRIVE_KI 1
#define DRIVE_KID 2000

#define DRIVE_TRACKING_KP 2//1
#define DRIVE_TRACKING_KPD 10//1
#define DRIVE_TRACKING_KI 0//1
#define DRIVE_TRACKING_KID 0//1
/////////////////////////////////////////////////////////////////////////////////////////
//
//
//                                     VARIABLES
//
//
//
/////////////////////////////////////////////////////////////////////////////////////////


 long accumulatedError[10]= {0,0,0,0,0,0,0,0,0,0};
 int leftEncoderPrevious = 0;
 int rightEncoderPrevious = 0;
 int stage = 1;
 int encoderPrevious[2] = {0,0};
 int speed[2] = {0,0};
 int cycleCount = 0;
 long grabPosition = 0;
/////////////////////////////////////////////////////////////////////////////////////////
//
//
//                                     PROTOTYPES
//
//
//
/////////////////////////////////////////////////////////////////////////////////////////
 int limitMotorPower (int number, int limit);
 void resetAccumulator();
/////////////////////////////////////////////////////////////////////////////////////////
//
//
//                                     FUNCTIONS
//
//
//
/////////////////////////////////////////////////////////////////////////////////////////

void drive(int left, int right)
{
  if (left > 127)
  {
   left = 127;
  }
  if (left < -127)
  {
   left = -127;
  }
  if (right > 127)
  {
    right = 127;
  }
  if (right < -127)
  {
   right = -127;
  }


 motor[port3] = motor[port5] = motor[port9] = left;
 motor[port2] = motor[port4] = motor[port8] = right;

}

/**************************************************************
*getPID
*Tells you what motor power to use in order to get to the target accurately.
*target:
*current:
*accumulatedError:
*kp:
*ki:
***************************************************************/
//
int getPID(int target,int current,int accumulatedslot, int kp,int kpd, int ki, int kid, int limit = 127)
{
  int outI = 0;
  int outP = 0;
  //==========================================P==============================================================
  int error  = target - current;
  //Error is target distance (target) - what we are at now :: Kp is the slope of the multiplier
  // If the distance from the target is large/small how much force it putout
  if (kp !=0 && kpd !=0)
{
  if (error > (127*kpd)/kp)
  {
       outP = 127;
  }
  else if (error < (-127*kpd)/kp)
  {
       outP = -127;
  }
  else
  {
      outP = kp*error/kpd;
  }
}
else
{
  outP = 0;
}
  //=============================================I===========================================================
  if (error < (127*kpd)/kp && error > (-127*kpd)/kp){
      accumulatedError[accumulatedslot] = accumulatedError[accumulatedslot] + error;
  }
  if (ki !=0 && kid !=0){
		  //If we haven't been on the target for awhile because P is too low. this will force the motors to go faster.
		  if (accumulatedError[accumulatedslot] > ((long)(127)*kid)/ki)
		  {
		      outI = 127;
		  }
		  else if (accumulatedError[accumulatedslot] < ((long)(-127)*kid)/ki)
		  {
		      outI = -127;
		  }
		  else
		  {
		       outI =ki*(int)(accumulatedError[accumulatedslot]/kid);
		  }
  }
  else{
      outI = 0;
  }



  //===========================================OUT PUT/RETURN================================================
  // If the output of I AND P are greater or less then 127 then it makes it +-127.
  return limitMotorPower(outP+outI, limit); //outI+outP);
}



/**************************************************************
*limitMotorPower
*If the output of I AND P are greater or less then 127 then it makes it +-127.
*number:
*limitMotorPower function is tested and working.
*
***************************************************************/
int limitMotorPower (int number, int limit)
{
	  if (number > limit)
	  {
	       return limit;
	  }
	  else if (number < -limit)
	  {
	       return limit * -1;
	  }
	  else
	  {
	      return number;
	  }
}

/**************************************************************
*withinRange
*
*a:
*target:
*distance:
***************************************************************/
bool withinRange(int a, int target, int distance)
{
	  if (a-target <= distance && a-target >= -distance){
	      return true;
	  }
	  else{
	      return false;
	  }
}

/**************************************************************
*driveStraight
*Drives Hal-e straight autonomously.
*target: Tells motors at what distance they should stop.
***************************************************************/
bool driveStraight (int target)
{
	  //drives the each side 2,1 (left). 3,4 (right). Using the PID which calulates and returns a number.
	  int l = getPID(SensorValue[rightEncoder],SensorValue[leftEncoder],3,DRIVE_TRACKING_KP,DRIVE_TRACKING_KPD,DRIVE_TRACKING_KI,DRIVE_TRACKING_KID);
	  int r = getPID(SensorValue[leftEncoder],SensorValue[rightEncoder],4,DRIVE_TRACKING_KP,DRIVE_TRACKING_KPD,DRIVE_TRACKING_KI,DRIVE_TRACKING_KID);
	  int left =limitMotorPower (  getPID(target,SensorValue[leftEncoder],1,DRIVE_KP,DRIVE_KPD,DRIVE_KI,DRIVE_KID)+ l, 80   );
	  int right=limitMotorPower (  getPID(target,SensorValue[rightEncoder],2,DRIVE_KP,DRIVE_KPD,DRIVE_KI,DRIVE_KID)+ r, 80  );
	  drive(left, right);
	  //==================================== Return True  if the bot is at its target===========================
	  bool ret = false;
	  if (withinRange (SensorValue[rightEncoder], target, ENCODERACCEPTANCERANGE) && withinRange(SensorValue[leftEncoder], target, ENCODERACCEPTANCERANGE) && withinRange(speed[0], 0, SPEEDACCEPTANCERANGE) && withinRange(speed[1], 0, SPEEDACCEPTANCERANGE))
	  {
	      ret = true;
	  }
	  leftEncoderPrevious = SensorValue[leftEncoder];
	  rightEncoderPrevious = SensorValue[rightEncoder];
	  accumulatedError = 0;
	  return ret;
}


/**************************************************************
*driveTurn
*Turns Hal-e during autonomous.
*degree: Tells motors at what distance they should stop.
*Turn is measured in degrees of turn from the center of the bot & 0 being straight forward, positive numbers turn it to the left.
***************************************************************/

bool driveTurn (int degree)
{
	  //Calculations assume Bot radius=8", 1 wheel rotation = 12.56", 1 encoder rotation=90 clicks
	  int target  = degree;
	  int left =limitMotorPower (getPID(-target,SensorValue[leftEncoder],1,DRIVE_KP,DRIVE_KPD,DRIVE_KI,DRIVE_KID), 127);//+ 0*getPID(-SensorValue[rightEncoder],SensorValue[leftEncoder],3,0,1));
	  int right = limitMotorPower (getPID(target,SensorValue[rightEncoder],2,DRIVE_KP,DRIVE_KPD,DRIVE_KI,DRIVE_KID), 127);//+ 0*getPID(-SensorValue[leftEncoder],SensorValue[rightEncoder],4,0,1));
	  drive(left, right);
	  bool ret = 0;
	  if (withinRange(SensorValue[rightEncoder] ,target,ENCODERACCEPTANCERANGE) && withinRange(SensorValue[leftEncoder] , -target,ENCODERACCEPTANCERANGE) && withinRange(speed[0], 0, ENCODERACCEPTANCERANGE) && withinRange(speed[1], 0, ENCODERACCEPTANCERANGE))
	  {
	    ret = true;
	  }
	  leftEncoderPrevious = SensorValue[leftEncoder];
	  rightEncoderPrevious = SensorValue[rightEncoder];
	  accumulatedError = 0;
	  return ret;
}
/**************************************************************
*calculateDriveSpeed
*Calculates how fast the robot is going, and saves it in the global array "speed".
****************************************************************/
void calculateDriveSpeed()
{
	  cycleCount++;
	  if(time1[T1] > 0 && cycleCount%15==0)
		{
			  int distance[2] = {SensorValue[leftEncoder] - encoderPrevious[0],SensorValue[rightEncoder] - encoderPrevious[1]};
			  for(int i = 0; i < 2; i++)
			  {
			      speed[i] = 10*distance[i] / time1[T1];
			  }
			  encoderPrevious[0] = SensorValue[leftEncoder];
			  encoderPrevious[1] = SensorValue[rightEncoder];
			  ClearTimer(T1);
    }
}

/**************************************************************
*liftArm
*Lifts up Hal-e's arm.
*desiredPosition: The arm's goal; where it is supposed to move to.
****************************************************************/
bool liftArm(int desiredPosition)
{
    motor[port1] = motor[port6] = motor[port10] = getPID(desiredPosition, SensorValue[armEncoder], 5, 15, 4, 0, 100);
    return(withinRange (SensorValue[armEncoder], desiredPosition, 8));
}

/**************************************************************
*grabber
*Opens or closes Hal-e's grabber.
*isOpen: Determines whether we want the grabber to open.
*grabTime:
****************************************************************/
bool grab(int grabDesired)
{
		    motor[port7] = -getPID(grabDesired, SensorValue[grabPotentiometer], 6, 1, 3, 0, 1000);
		    return(withinRange (SensorValue[grabPotentiometer], grabDesired, 200));
}

/**************************************************************
*twentyPtAuto
*autonomous to score 20  points.
***************************************************************/
//arm 0 - 800

void nearAuto(isRed)
{

    /********************Score four tubes.**********************/
	  if (stage == 1)
	  {
	      //In which the robot lifts its arm.
	      bool x = true;
	      bool y = liftArm(340);
	      bool z = true;


	      if (x && y && z)
	      {
			      stage++;
			      SensorValue[rightEncoder]=SensorValue[leftEncoder]=0;
			      resetAccumulator();
	      }
	  }

	  if (stage == 2)
	  {
	      //In which the robot opens its grabber.
	      bool x = true;
	      bool y = true;
	      bool z = grab(2000);

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 3)
	  {
	      //In which the robot lowers its arm.
	      bool x = true;
	      bool y = liftArm(0);
	      bool z = true;
	      motor[port7] = 0;

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 4)
	  {
	      // In which the robot drives forward to the tubes.
	      bool x = driveStraight(2800);
	      bool y = true;
	      bool z = true;
	      motor[port1] = motor[port6] = motor[port10] = 0;

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 5)
	  {
	      //In which the robot grabs the tubes.
	      bool x = true;
	      bool y = true;
	      bool z = (SensorValue[grabPotentiometer] > 2700);
	      motor[port7] = -127;

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	          wait1Msec(1000);
	      }
	  }

	  if (stage == 6)
	  {
	      //In which the robot holds the tubes, lifts its arm, and turns to the goal.
	      bool x = driveTurn(isRed?1600:-1600);
	      bool y = liftArm(680);
	      bool z = true;
	      motor[port7] = -75;

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }

	  }

	  if (stage == 7)
	  {
	      //In which the robot drives to the goal.
	      bool x = driveStraight(6615);
	      bool y = liftArm(680);
	      bool z = true;
	      motor[port7] = -75;

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 8)
	  {
	      //In which the robot lowers the tubes onto the goal.
	      bool x = true;
	      bool y = liftArm(180);
	      bool z = true;
	      motor[port7] = -75;

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 9)
	  {
	      //In which the robot lets go of the tubes and backs up a bit.
	      bool x = driveStraight(-2500);
	      bool y = liftArm(180);
	      bool z = grab(2000);

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  /****************************************************************/

	  if (stage == 10)
	  {
	      //In witch the robot turns around.
	      bool x = driveTurn(isRed?9500:-9500);
	      bool y = liftArm(180);
	      bool z = true;

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 11)
	  {
	      //In which the robot adjusts its arm to allow for easy driving.
	      bool x = true;
	      bool y = liftArm(180);
	      bool z = grab(2000);

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 12)
	  {
	      //In which the robot drives up to the opponent's tubes.
	      bool x = driveStraight(6000);
	      bool y = liftArm(180);
	      bool z = grab(2000);

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 13)
	  {
	      //In which the robot activates sleep mode.
	      motor[port3] = motor[port5] = motor[port9] = 0;
	      motor[port2] = motor[port4] = motor[port8] = 0;
	      motor[port1] = motor[port6] = motor[port10] = 0;
	      motor[port7] = 0;
	  }

}

void farAuto(isRed)
{
    if (stage == 1)
	  {
	      //In which the robot lifts its arm.
	      bool x = true;
	      bool y = liftArm(340);
	      bool z = true;


	      if (x && y && z)
	      {
			      stage++;
			      SensorValue[rightEncoder]=SensorValue[leftEncoder]=0;
			      resetAccumulator();
	      }
	  }

	  if (stage == 2)
	  {
	      //In which the robot opens its grabber.
	      bool x = true;
	      bool y = true;
	      bool z = grab(2000);

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 3)
	  {
	      //In which the robot lowers its arm.
	      bool x = true;
	      bool y = liftArm(0);
	      bool z = true;
	      motor[port7] = 0;

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 4)
	  {
	      //In which the robot drives forward to the tubes.
	      bool x = driveStraight(6000);
	      bool y = liftArm(0);
	      bool z = true;
	      motor[port7] = 0;

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 5)
	  {
	      //In which the robot grabs the tubes.
	      bool x = true;
	      bool y = true;
	      bool z = (SensorValue[grabPotentiometer] > 2700);
	      motor[port1] = motor[port6] = motor[port10] = 0;
	      motor[port7] = -127;

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	          wait1Msec(1000);
	      }
	  }

	  if (stage == 6)
	  {
	      //In which the robot lifts up the tubes and turns to a point nearer the ladder.
	      bool x = driveTurn(isRed?-800:800);
	      bool y = liftArm(340);
	      bool z = true;
	      motor[port7] = -75;

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 7)
	  {
	      //In which the robot drives to a point nearer the ladder.
	      bool x = driveStraight(21000);
	      bool y = liftArm(340);
	      bool z = true;
	      motor[port7] = -75;

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 8)
	  {
	      //In which the robot turns toward the ladder.
	      bool x = driveTurn(isRed?-5400:5400);
	      bool y = liftArm(340);
	      bool z = true;
	      motor[port7] = -75;

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 9)
	  {
	      //In which the robot drives up to the ladder.
	      bool x = driveStraight(1900);
	      bool y = liftArm(340);
	      bool z = true;
	      motor[port7] = -75;

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 10)
	  {
	      //In which the robot drops the opponent's tubes under the ladder.
	      bool x = true;
	      bool y = liftArm(340);
	      bool z = grab(2000);

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 11)
	  {
	      //In which the robot backs up a bit from the ladder.
	      bool x = driveStraight(-2000);
	      bool y = liftArm(340);
	      bool z = true;
	      motor[port7] = 0;

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 12)
	  {
	      //In which the robot turns toward its tubes.
	      bool x = driveTurn(isRed?4000:-4000);
	      bool y = liftArm(340);
	      bool z = true;
	      motor[port7] = -75;

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	      }
	  }

	  if (stage == 13)
	  {
	      //In which the robot lowers its arm.
	      bool x = true;
	      bool y = liftArm(0);
	      bool z = true;
	      motor[port7] = -75;

	      if (x && y && z)
	      {
	          stage++;
	          SensorValue[rightEncoder] = SensorValue[leftEncoder] = 0;
	          resetAccumulator();
	          wait1Msec(1000);
	      }
	  }

	  if (stage == 14)
	  {
	      //In which the robot activates sleep mode.
	      motor[port3] = motor[port5] = motor[port9] = 0;
	      motor[port2] = motor[port4] = motor[port8] = 0;
	      motor[port1] = motor[port6] = motor[port10] = 0;
	      motor[port7] = 0;
	  }
}

void resetAccumulator()
{
	  for(int i = 0; i < 10; i++)
	  {
	      accumulatedError[i] = 0;
	  }
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
  while (true)
  {
      bool isNear = SensorValue[dgtl11];
	    calculateDriveSpeed();
	    //TODO: autonjmprs
	    if (isNear)
	    {
	        nearAuto(SensorValue[dgtl12]);
	    }
	    else
	    {
	        farAuto(SensorValue[dgtl12]);
	    }
  }
  // .....................................................................................
//twentyPtAuto();
  //drive(60,60);
  // .....................................................................................

	//AuonomousCodePlaceholderForTesting();  // Remove this function call once you have "real" code.
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
	// User control code here, inside the loop

	while (true)
	{
	  // This is the main execution loop for the user control program. Each time through the loop
	  // your program should update motor + servo values based on feedback from the joysticks.

	  // .....................................................................................
	  // Insert user code here. This is where you use the joystick values to update your motors, etc.

	  //runs the drive
	  //left 3, 5, 9
	  //right 2, 4, 8
	  if((vexRT[Ch3] + vexRT[Ch4]) < 10 && (vexRT[Ch3] + vexRT[Ch4]) > -10){
	      motor[port3] = motor[port5] = motor[port9] = 0;
	  }

	  else{
	      motor[port3] = motor[port5] = motor[port9] = vexRT[Ch3] + vexRT[Ch4];
	  }
//
	  if((vexRT[Ch3] - vexRT[Ch4]) < 10 && (vexRT[Ch3] - vexRT[Ch4]) > -10){
	      motor[port2] = motor[port4] = motor[port8] = 0;
	  }

	  else{
	      motor[port2] = motor[port4] = motor[port8] =  vexRT[Ch3] - vexRT[Ch4];
    }

    //runs the arm
    //left lift 1, 6
	  //right lift 10
	  motor[port1] = motor[port6] = motor[port10] = vexRT[Ch2];

	  //runs the grabber
	  //grabber 7
	  if(vexRT[Btn6U] == 1){
	      motor[port7] = 127;
	  }

	  else if(vexRT[Btn6D] == 1){
	      motor[port7] = -127;
	  }

	  else{
	      motor[port7] = 0;
	  }


	  // .....................................................................................
	}
}
